#!/usr/bin/env python3

import pdb
import math
import numpy as np
from scipy import interpolate
from shapely.geometry import Polygon, LineString, Point, MultiLineString,MultiPoint
import matplotlib.pyplot as plt
import bezier_curve_algorithm as ba

def is_close(p1, p2, rel_tol=0.0, abs_tol=2.0):
    x1, y1 = p1
    x2, y2 = p2
    return math.isclose(x1, x2, rel_tol=rel_tol, abs_tol=abs_tol) and \
           math.isclose(y1, y2, rel_tol=rel_tol, abs_tol=abs_tol)


#计算曲线与多边形边线的交点
def curve_intersect_polygon(_curve, _polygon):
    #将曲线转换成曲线折线段
    polyline = LineString([_curve.interpolate(i/10.0, normalized=True) for i in range(11)])
    # 遍历多边形边界的所有线段，查找直线和多边形边的交点
    total_inter_points = []
    x_list =[]
    y_list =[]
    coords = _polygon.exterior.coords
    for i in range(len(coords)-1):
        # 创建一个表示多边形边的线段
        edge_start = coords[i]
        edge_end = coords[i+1]
        #print("edge_start:",edge_start, "edge_end:",edge_end)
        seg = LineString([edge_start, edge_end])
        # 检查直线和多边形边的交点
        intersection = seg.intersection(polyline)
        if not intersection.is_empty and isinstance(intersection, Point):
            total_inter_points.append(intersection)
        elif not intersection.is_empty and isinstance(intersection, MultiPoint):
            for pos in intersection.geoms:
                total_inter_points.append(pos)
    for pos in total_inter_points:
        x_list.append(pos.x)
        y_list.append(pos.y)
    return total_inter_points, x_list, y_list

# 定义正方形的四个顶点坐标
vertices = [(0, 0), (-20, 70), (70, 90), (90, 70), (70,0)]
# 创建正方形对象
poly = Polygon(vertices)

# 画出多边形的边线
x, y = poly.exterior.coords.xy
plt.plot(x, y)

# 画出一条直线，计算出与多边形的交点
line = LineString([(-40, 30), (120, 35)])
x, y = line.coords.xy
#plt.plot(x, y)
#画出与多边形的交点
total, x, y = curve_intersect_polygon(line, poly)
#plt.plot(x, y)

#构造贝塞尔曲线
p0 = ba.Point(x[0], y[0])
p1 = ba.Point(20,200)
p2 = ba.Point(60,40)
p3 = ba.Point(x[1], y[1])
max_num = 30
bezier_curve, x_list, y_list = ba.cubic_bezier_curve(p0, p1, p2, p3, max_num)
#将贝塞尔曲线结构点转换成二维数组

bc_points = np.array([x_list, y_list])
bc_points = bc_points.T
# 绘制原曲线，未拟合多边形
plt.plot(bc_points[:, 0],bc_points[:, 1])

# 基于已有的贝塞尔曲线点生成LineString
bezier_curve_linestring = LineString(bc_points)

# 判断曲线是否完全多边形内，如果不是，以交点为控制点，重新生成贝塞尔曲线
if bezier_curve_linestring.within(poly):
    print("完全在多边形内")
else:
    print("曲线不完全在多边形内")

loop = 0
new_curve = bezier_curve_linestring
while False == new_curve.within(poly) and loop < 1:
    intersection, x_list, y_list = curve_intersect_polygon(new_curve, poly)
    #删除p0，p3
    print("loop:", loop, "曲线与多边形交点数：", len(intersection))
    print(intersection)
    for pos in intersection:
        tmp_pos = ba.Point(pos.x, pos.y)
        if is_close(tmp_pos, p0) or is_close(tmp_pos, p1):
            intersection.remove(pos)
    #排序
    intersection = sorted(intersection, key=lambda p: p.coords[0])
    #使用剩余的交点作为控制点
    ip_num = len(intersection)
    x_list = []
    y_list = []
    if 1== ip_num:#二次贝塞尔
        p1 = ba.Point(intersection[0].x, intersection[0].y)
        print("p0:", p0)
        print("p1:", p1)
        print("p3:", p3)
        curve, x_list, y_list = ba.quadratic_bezier_curve(p0, p1, p3, max_num)
    elif 1 < ip_num:#三次贝塞尔
        p1 = ba.Point(intersection[0].x, intersection[0].y)
        p2 = ba.Point(intersection[1].x, intersection[2].y)
        print("p0:", p0)
        print("p1:", p1)
        print("p2:", p2)
        print("p3:", p3)
        curve, x_list, y_list = ba.cubic_bezier_curve(p0, p1, p2,  p3, max_num)
    else:
        break
    bc_points = np.array([x_list, y_list])
    bc_points = bc_points.T
    # 基于已有的贝塞尔曲线点生成LineString
    new_curve = LineString(bc_points)
    loop = loop + 1

#获取拟合后的曲线的二维数组
# 获取 LineString 的坐标数组
coords = list(new_curve.coords)
print("final curve:", len(coords))
x, y = zip(*coords)
line_array = [[x[i], y[i]] for i in range(len(coords))]

# 绘制处理后的曲线,已拟合到多边形内的
plt.plot(x, y)
plt.show()
